package ai.astar;

import java.util.Enumeration;
import java.util.Hashtable;
import java.util.PriorityQueue;
import java.util.Stack;

public class DirectedGraph {
    private Hashtable<String,Node> allNodes;
    private PriorityQueue<Node> nextNodesQueue;
    
    public DirectedGraph(Hashtable<String,Node> allNodes) {
    	this.allNodes = allNodes;
	}
    
    public Stack<Node> aStar(String startNodeString, String targetNodeString){
    	NodeDistanceComparator comparator = new NodeDistanceComparator();
		nextNodesQueue = new PriorityQueue<Node>(allNodes.size(), comparator);
    	
    	Enumeration<String> keys = allNodes.keys();
		while( keys.hasMoreElements() ) {
			Object key = keys.nextElement();
			Node node = (Node) allNodes.get(key);
			node.setDistance(Double.MAX_VALUE);
			node.setRankingDistance(Double.MAX_VALUE);
			node.setPredecessorNode(null);
			nextNodesQueue.add(node);
		}
		Node currentNode = allNodes.get(startNodeString);
		currentNode.setDistance(0);
		currentNode.setRankingDistance(0);
		Node targetNode = allNodes.get(targetNodeString);
		while(currentNode != targetNode){
			expandGraph(currentNode, targetNode);
			currentNode = nextNodesQueue.remove();
		}
		return reconstructPath(targetNode);
    }

	private void expandGraph(Node currentNode, Node targetNode) {
		for (Link link: currentNode.getNeighbors()) {
			double distance = currentNode.getDistance() + link.getLength();
			Node nextNode = link.getTo();
			if(nextNode == targetNode){
				System.out.println("nextNode == targetNode");
			}
			if(distance < nextNode.getDistance()){
				nextNode.setDistance(distance);
				nextNode.setRankingDistance(distance+euclidDistance(nextNode, targetNode));
				nextNode.setPredecessorNode(currentNode);
				nextNodesQueue.remove(nextNode);
				nextNodesQueue.add(nextNode);
			}
		}
	}

	private Stack<Node> reconstructPath(Node targetNode) {
		Stack<Node> path = new Stack<Node>();
		Node currentNode = targetNode;
		while(currentNode.getPredecessorNode() != null){
			path.add(currentNode);
			currentNode = currentNode.getPredecessorNode();
		}
		path.add(currentNode);
		return path;
	}
	
	private double euclidDistance(Node node1, Node node2){
		return Math.sqrt(Math.pow(node2.getX()-node1.getX(),2)+Math.pow(node2.getY()-node1.getY(),2));
	}
}
